import math

class Matrix:
    """4x4 row major matrix"""
    
    def __init__(self):
        self.v = [[0 for i in range(4)] for j in range(4)]
        self.v[0][0] = self.v[1][1] = self.v[2][2] = self.v[3][3] = 1

    def __repr__(self):
        return "<Matrix %s>" % str(self.v)

    def __mul__(self, m):
        if isinstance(m,Matrix):
            result = Matrix()
            for i in range(4):
                for j in range(4):
                    for k in range(4):
                        result.v[i][j] += self.v[i][k] * m.v[k][j]
            return result

        elif isinstance(m,Vec3):
            return Vec3(self.v[0][0] * m.x + self.v[0][1] * m.y + self.v[0][2] * m.z + self.v[0][3],
                        self.v[1][0] * m.x + self.v[1][1] * m.y + self.v[1][2] * m.z + self.v[1][3],
                        self.v[2][0] * m.x + self.v[2][1] * m.y + self.v[2][2] * m.z + self.v[2][3])

        else:
            raise RuntimeError

    @staticmethod
    def makeTranslation(v):
        m = Matrix()
        m.v[0][3] = v.x
        m.v[1][3] = v.y
        m.v[2][3] = v.z
        return m

    @staticmethod
    def makeScale(v):
        m = Matrix()
        m.v[0][0] = v.x
        m.v[1][1] = v.y
        m.v[2][2] = v.z
        return m

    @staticmethod
    def makeXRotation(angle):
        m = Matrix()
        m.v[1][1] = math.cos(angle)
        m.v[1][2] = -math.sin(angle)
        m.v[2][1] = math.sin(angle)
        m.v[2][2] = math.cos(angle)
        return m

    @staticmethod
    def makeYRotation(angle):
        m = Matrix()
        m.v[0][0] = math.cos(angle)
        m.v[0][2] = math.sin(angle)
        m.v[2][0] = -math.sin(angle)
        m.v[2][2] = math.cos(angle)
        return m

    @staticmethod
    def makeZRotation(angle):
        m = Matrix()
        m.v[0][0] = math.cos(angle)
        m.v[0][1] = -math.sin(angle)
        m.v[1][0] = math.sin(angle)
        m.v[1][1] = math.cos(angle)
        return m

    @staticmethod
    def makeRotation(q):
        q = q.inverse() #WHAT
        
        xx = q.x * q.x
        xy = q.x * q.y
        xz = q.x * q.z
        xw = q.x * q.w
        yy = q.y * q.y
        yz = q.y * q.z
        yw = q.y * q.w
        zz = q.z * q.z
        zw = q.z * q.w

        m = Matrix()
        m.v[0][0] = 1 - 2 * (yy + zz)
        m.v[0][1] = 2 * (xy + zw)
        m.v[0][2] = 2 * (xz - yw)
        m.v[1][0] = 2 * (xy - zw)
        m.v[1][1] = 1 - 2 * (xx + zz)
        m.v[1][2] = 2 * (yz + xw)
        m.v[2][0] = 2 * (xz + yw)
        m.v[2][1] = 2 * (yz - xw)
        m.v[2][2] = 1 - 2 * (xx + yy)
        
        return m
    
Matrix.I = Matrix()

from quat import Quat
from vec3 import Vec3

if __name__ == "__main__":
    a = -math.pi/4
    
    x1 = Matrix.makeRotation(Quat.fromAxisAngle(Vec3.UNIT_X,a))
    x2 = Matrix.makeXRotation(a)
    print x1
    print x2
    print
    
    y1 = Matrix.makeRotation(Quat.fromAxisAngle(Vec3.UNIT_Y,a))
    y2 = Matrix.makeYRotation(a)
    print y1
    print y2
    print
    
    z1 = Matrix.makeRotation(Quat.fromAxisAngle(Vec3.UNIT_Z,a))
    z2 = Matrix.makeZRotation(a)
    print z1
    print z2
    print
    
    m1 = x1 * y1 * z1
    m2 = x2 * y2 * z2
    print m1
    print m2
